﻿#define UNROLL

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Warburton.FastBitArray.Factories;

namespace Warburton.FastBitArray.Implementations
{
    unsafe class Unsafe64BitBitArray
        : IBitArray
    {
        private Factories.Unsafe64BitFactory _factory;
        private int _length;
        private ulong[] _bits;

        public Unsafe64BitBitArray(Unsafe64BitFactory factory, int length)
        {
            _factory = factory;
            _length = length;
            if (_length > 0)
                _bits = new ulong[((length - 1) / 64) + 1];
        }

        public IBitArray Set(int index, bool value)
        {
            if (_length == 0 || index >= _length)
                throw new IndexOutOfRangeException();

            var pos = index >> 6;
            var bit = index & 0x3F;

            var bitPos = 1UL << bit;
            if (value)
                _bits[pos] |= bitPos;
            else
                _bits[pos] &= ~bitPos;

            return this;
        }

        public IBitArray SetAll(bool value)
        {
            if (_length > 0)
            {
                var v = value ? 0xFFFFFFFFFFFFFFFF : 0;
                unsafe
                {
                    var count = _bits.Length;
                    fixed (ulong* bits = &_bits[0])
                    {
                        int i = 0;
#if UNROLL
                        for (; i < count - 4; i += 4)
                        {
                            *(bits + i + 0) = v;
                            *(bits + i + 1) = v;
                            *(bits + i + 2) = v;
                            *(bits + i + 3) = v;
                        }
#endif

                        for (; i < count; ++i)
                            *(bits + i) = v;
                    }
                }
            }
            return this;
        }

        public IBitArray And(IBitArray value)
        {
            var rhs = value as Unsafe64BitBitArray;
            if (rhs == null)
                throw new ArgumentException();
            if (rhs._length != _length)
                throw new ArgumentException();

            unsafe
            {
                var count = _bits.Length;
                fixed (ulong* p1 = &_bits[0])
                fixed (ulong* p2 = &rhs._bits[0])
                {
                    int i = 0;
#if UNROLL
                    for (; i < count-4; i += 4)
                    {
                        *(p1 + i + 0) &= *(p2 + i + 0);
                        *(p1 + i + 1) &= *(p2 + i + 1);
                        *(p1 + i + 2) &= *(p2 + i + 2);
                        *(p1 + i + 3) &= *(p2 + i + 3);
                    }
#endif
                    for (; i < count; ++i)
                        *(p1 + i) &= *(p2 + i);
                }
            }

            return this;
        }

        public IBitArray Or(IBitArray value)
        {
            var rhs = value as Unsafe64BitBitArray;
            if (rhs == null)
                throw new ArgumentException();
            if (rhs._length != _length)
                throw new ArgumentException();

            unsafe
            {
                var count = _bits.Length;
                fixed (ulong* p1 = &_bits[0])
                fixed (ulong* p2 = &rhs._bits[0])
                {
                    int i = 0;
#if UNROLL
                    for (; i < count - 4; i += 4)
                    {
                        *(p1 + i + 0) |= *(p2 + i + 0);
                        *(p1 + i + 1) |= *(p2 + i + 1);
                        *(p1 + i + 2) |= *(p2 + i + 2);
                        *(p1 + i + 3) |= *(p2 + i + 3);
                    }
#endif
                    for (; i < count; ++i)
                        *(p1 + i) |= *(p2 + i);
                }
            }

            return this;
        }

        public IBitArray Xor(IBitArray value)
        {
            var rhs = value as Unsafe64BitBitArray;
            if (rhs == null)
                throw new ArgumentException();
            if (rhs._length != _length)
                throw new ArgumentException();

            unsafe
            {
                var count = _bits.Length;
                fixed (ulong* p1 = &_bits[0])
                fixed (ulong* p2 = &rhs._bits[0])
                {
                    int i = 0;
#if UNROLL
                    for (; i < count - 4; i += 4)
                    {
                        *(p1 + i + 0) ^= *(p2 + i + 0);
                        *(p1 + i + 1) ^= *(p2 + i + 1);
                        *(p1 + i + 2) ^= *(p2 + i + 2);
                        *(p1 + i + 3) ^= *(p2 + i + 3);
                    }
#endif
                    for (; i < count; ++i)
                        *(p1 + i) ^= *(p2 + i);
                }
            }

            return this;
        }

        public IBitArray Not()
        {
            if (_length > 0)
            {
                unsafe
                {
                    var count = _bits.Length;
                    fixed (ulong* p1 = &_bits[0])
                    {
                        var i = 0;
#if UNROLL
                        for (; i < count - 4; i += 4)
                        {
                            *(p1 + i + 0) = ~(*(p1 + i + 0)); 
                            *(p1 + i + 1) = ~(*(p1 + i + 1));
                            *(p1 + i + 2) = ~(*(p1 + i + 2));
                            *(p1 + i + 3) = ~(*(p1 + i + 3));
                        }
#endif
                        for (; i < count; ++i)
                            *(p1 + i) = ~(*(p1 + i));
                    }
                }
            }
            return this;
        }

        public IBitArrayFactory Factory
        {
            get { return _factory; }
        }

        public int NumberOfBits
        {
            get { return _length; }
        }

        public bool Get(int index)
        {
            if (_length == 0 || index >= _length)
                throw new IndexOutOfRangeException();

            var pos = index >> 6;   // "/ 64"
            var bit = index & 0x3F; // "% 64"

            var bitPos = 1UL << bit;
            return (_bits[pos] & bitPos) == bitPos;
        }

        public IEnumerable<int> GetIndexes(bool value)
        {
            var bitPos = 0UL;
            var index = 0;
            var current = 0UL;

            for (int i = 0; i < _length; ++i)
            {
                if (bitPos > 0)
                    bitPos <<= 1;
                else
                {
                    current = _bits[index];
                    bitPos = 1;
                    ++index;
                }
                var bit = (current & bitPos);
                if ((bit == 0 && !value) || (bit != 0 && value))
                    yield return i;
            }
        }
    }
}
